#ifndef ARAL_INTERFACE_PLANNER_HPP
#define ARAL_INTERFACE_PLANNER_HPP

#include <string>
#include <vector>
#include <functional>
#include "aral/data_structure_definition.hpp"

namespace ARAL::interface {

//机器人规划器  
class Planner
{
protected:
    virtual ~Planner() = default;

public:
    /**
     * @brief 设置规划器的容量信息
     * @param type: 规划器的容量类型
     *        LookAhead: 路径前瞻段数，在动态交融功能中使用，如果用户插入到路径缓冲队列中的路径段数大于前瞻段数时，即对插入的多段路径进行速度规划生成待执行的轨迹段
     *        ExecutionTrajectory: 轨迹执行队列的容量:指轨迹点个数，当实际轨迹点个数大于规定容量时，用户不应该再插入路径(保证计算效率)，否则会插入失败，并打印警告日志
     * @param size: 具体的大小
     * @return: 是否设置成功
     */
    virtual int tpSetPlannerCapacity(const PlannerCapacity& type, const unsigned int& size) = 0;

    /**
     * @brief 返回规划器具体的容量信息
     * @return: 具体size大小
     */
    virtual unsigned int tpGetPlannerCapacity(const PlannerCapacity& type)const = 0;

    /**
     * @brief 返回规划器内部队列的长度
     * @param queue: 规划器队列类型
     *        ExecutionQueue: 轨迹执行队列，获取轨迹队列待执行的轨迹点数, 如果返回值 > 0, 则表示可以通过tpUpdateCycle接口获取轨迹点
     *        BufferQueue:    路径缓冲队列，用于获取队列中剩余的路径段数，只包含用户插入的，不包括交融路径。(路径缓冲队列:需要交融的路径会先压入该队列,
     *                        当结束交融或者该队列的长度大于等于前瞻段数，算法会对缓冲队列中的路径进行轨迹规划，并将规划的结果压入轨迹执行队列)
     *        PathQueue:      返回用户传入给规划器的还未执行的路径段数
     * @return 具体的段数信息
     */
    virtual int tpGetPlannerDepth(const PlannerQueue& queue)const = 0;

    /**
     * @brief 得到指定ID对应的路径段的运行时长，如果pathId = -1, 则返回所有路径段运动总时长
     * @param pathId: 路径段ID
     * @return: 运动时长，单位:秒
     */
    virtual double tpGetMoveDuration(const double& pathId)const = 0;

    /**
      * @brief 获得轨迹队列剩余轨迹段运动总时长，从当前轨迹点开始
      */
    virtual double tpGetPlannerLeftMoveDuration() const = 0;

    /**
      * @brief 获得暂停点信息.如果是关节空间暂停,则只返回关节角；如果是在笛卡尔空间暂停,则返回位姿和关节角.
      * @param point: 暂停点信息
      * @return: 返回值 < 0 表示获取暂停点失败(求逆解失败).
      */
    virtual int tpGetPausedPoint(TrajectoryPoint& point, const Array2d& ik_eps = {ARAL_IK_ITER_POS_EPS, ARAL_IK_ITER_ORI_EPS}) = 0;

    /**
     * @brief 获取机械臂未来一些时刻的路点信息, 每个路点包含关节角和笛卡尔空间位姿信息
     *  tpGetPathPointAtGivenTime 和 tpUpdateCycle 两个函数的采样起始时刻相等,
     *  tpGetPathPointAtGivenTime 是从采样起始时刻往后看指定一段时间的轨迹状态, 函数返回后不会更新采样的起始时刻;
     *  tpUpdateCycle 函数也是从采样起始时刻往后看指定一段时间的轨迹状态, 函数返回后会更新采样的起始时刻。
     * @param sample_period: 采样时间间隔
     * @param n: 获取未来 n 个以sample_period为时间间隔的路点
     * @param points: 路点
     * @return: 返回值 < 0 表示获取路点信息失败.
     */
    virtual int tpGetPathPointAtGivenTime(const double& sample_period, const unsigned int& n, std::vector<PathPoint>& points) const = 0;

    /**
     * @brief 得到规划器在某条路径上对应比例的路径点
     * @param path_id: 路径ID
     * @param s: 路径弧长比例, 取值范围[0, 1]
     * @param point: 路径点
     * @return if < 0, 则表示计算出错
     */
    virtual int tpGetPathPointAtGivenPathLengthRatio(const int& path_id, const double& s, PathPoint& point)const = 0;

    /**
     * @brief 规划器在某条路径上以分辨率长度插值得到的一系列路径点
     * @param path_info: 描述多条路径的几何信息，路径属性，运动属性以及工具和工件信息，其每个元素 MotionProperty 用于创建一条基本路径（直线、圆弧、样条...）,
     *                   通过在PathProperty设置交融半径大小，路径之间可进行交融形成组合路径(目前支持直线、圆弧和关节之间的交融）
     * @param s: 分辨率长度, 单位m
     * @param interPoints: 插值的路径点
     * @return if < 0, 则表示计算出错
     */
    virtual int tpGetPathPointAtGivenResolutionLength(const std::vector<MotionProperty>& path_info, const double& s, std::vector<PathPoint>& interPoints) const = 0;

    /**
     * @brief 向规划器中添加路点, 实时调整目标点(当前的目标点被更新后，不能保证达到之前设定的目标点)
     *        当用户不指定达到目标点的运动时间时, 算法会根据运动约束计算最短的达到时间
     * @param point: 具体的路点信息, 如果point的类型为关节空间, 则跟踪关节空间的目标点; 如果point的类型为笛卡尔空间, 则跟踪笛卡尔空间的速度。(实际的需求一般只对应这两种功能)
     * @param move_property: 运动属性(目标跟踪没有明确的路径, 所以该功能属于速度级运动)
     * @param tool_workpiece: 工具和工件信息
     * @param buffSize: 轨迹执行队列中需要缓冲的轨迹点数，用于对冲调用该接口需要的耗时.(如果软件已经调用tpUpdateCycle在软件层缓存轨迹点，则buffSize可置为0)
     * @param verbose: 该值为true时表示显示算法的计算过程信息, 为false时会屏蔽该信息.
     * @return: 返回值 < 0 表示添加失败
     */
    virtual int tpTargetMotionTracking(const PathPoint& point, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece, const unsigned int buffSize, const bool verbose = true) = 0;

    /**
     * @brief 动态轨迹跟踪，能保证经过所有目标点(如果用户没有指定目标点的速度或加速度，则算法内部会进行状态估计), 目前只支持关节空间轨迹点
     *        如果需要结束轨迹跟踪模式，需要显式将 status 置为 TrajectoryTrackingStatus::End.
     * @param point: 跟踪的关键点, 需要指定ID信息
     * @param move_property: 运动属性信息(运动约束条件在该结构体中进行设置, 轨迹跟踪没有明确的路径, 所以该功能属于速度级运动)
     *        用户必须显示指定目标点的运动时间间隔, 且必须大于插补周期(5ms)
     * @param tool_workpiece: 工具和工件信息
     * @param smooth_scale: 运动平滑程度, 设置范围(0, 1], 1表示平滑性最好
     * @param delay_sacle: 运动响应速度, 设置范围(0, 1], 1表示跟踪响应最快
     * @param status: 跟踪状态
     *        TrajectoryTrackingStatus::Track: 跟踪;
     *        TrajectoryTrackingStatus::End: 结束轨迹跟踪模式,如果当前目标点速度加速度不为0,则会自动规划减速到0的运动(如果在减速过程中用户提供了新的目标点,
     *                                       则会结束减速,继续跟踪新的目标点).
     * 如果当前处于轨迹跟踪模式, 必须显式调用结束指令(即把status标志置成TrajectoryTrackingStatus::End)才能开启别的运动.
     * @return 累计的运动延迟时间(累计的实际运动时间 - 累计的运动期望时间); 如果返回值 < 0 表示计算失败
     */
    virtual double tpTrajectoryTracking(const PathPoint& point, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece, const double& smooth_scale, const double& delay_sacle, const TrajectoryTrackingStatus& status) = 0;

    /**
     * @brief 向规划器中添加直线运动(关节空间或笛卡尔空间)
     * @param point: 末端目标位姿或关节构型
     * @param path_property: 直线路径的几何属性
     * @param move_property: 直线路径的运动属性
     * @param tool_workpiece: 工具和工件信息
     * @return: 返回值 < 0 表示添加失败, 具体定义查询 return_value_definition.hpp
    */
    virtual int tpAddPositionLine(const PathPoint& point, const PathProperty& path_property, const MoveProperty& move_property) = 0;

    /**
     * @brief 向规划器中添加多点运动路径, 具体的类型参考PathProperty.CurveProperty.type(关节空间或笛卡尔空间)
     * @param points: 描述路径几何信息的路径点(id指的第一段路径的id, 后面的id依次递增)
     * @param path_property: 路径对应的几何属性
     * @param move_property: 路径对应的运动属性
     * @param tool_workpiece: 工具和工件信息
     * @return: 返回值 < 0 表示添加失败, 具体定义查询 return_value_definition.hpp
     */
    virtual int tpAddPoints(const std::vector<PathPoint>& points, const PathProperty& path_property, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece) = 0;

    /**
     * @brief 规划生成无碰撞路径，目前仅支持关节空间
     * @param in_points: 描述路径几何信息的 N 个路径点(N >= 2)
     * @param path_property: 路径对应的几何属性
     * @param obstacle_avoidance_params: 避障属性，包括避障算法类型、规划步长以及是否使能路径简化功能
     * @param tool_workpiece: 工具和工件信息
     * @param out_points: 规划输出的无碰撞路径点
     * @return: 返回值 < 0 表示添加失败, 具体定义查询return_value_definition.hpp
     */
    virtual int tpGenerateCollisionFreePoints(const std::vector<PathPoint>& in_points, const ObstacleAvoidanceParameters& obstacle_avoidance_params, const ToolWorkpiece& tool_workpiece, std::vector<PathPoint>& out_points) = 0;

    /**
     * @brief 向规划器中添加速度运动（关节空间或笛卡尔空间）
     * @param id: 速度运动对应的轨迹id
     * @param DescribeSpace: 运动空间属性
     * @param move_property: 路点对应的运动属性
     * @param tool_workpiece: 工具和工件信息
     * @return: 返回值 < 0 表示添加失败，具体定义查询return_value_definition.hpp
     */
    virtual int tpAddVelocityLine(const int& id, const DescribeSpace& space, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece) = 0;

    /**
     * @brief 显式指出以当前轨迹段为界, 终止交融
     * @return: 返回值 < 0 表示添加失败
     */
    virtual int tpSetEndPath() = 0;

    /**
     * @brief 设置规划器恢复起始点
     * @param from: 恢复点
     * @param path_property: 路径属性
     * @param move_property: 运动属性
     * @return: 返回值 < 0 表示添加失败
     */
    virtual int tpSetResumePoint(const PathPoint& from, const PathProperty& path_property, const MoveProperty& move_property) = 0;


};//class Planner     
typedef std::shared_ptr<Planner> PlannerPtr;
}   //namespace ARAL::interface

#endif // ARAL_INTERFACE_PLANNER_HPP